/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;

/**
 *
 * @author Robotics
 */
public class RobotDrivePlus extends RobotDrive {
	
	/**
     * RobotDrive constructor with SpeedControllers as parameters. Use this method if the
     * speed controllers are not plugged into the default digital sidecar
     * @param leftMotor Left motor port
     * @param rightMotor Right motor port
     */
    public RobotDrivePlus(SpeedController leftMotor, SpeedController rightMotor) {
        super(leftMotor, rightMotor, kDefaultSensitivity);
    }

	/**
     * Constructor for RobotDrive with 4 motors specified as SpeedController objects.
     * Speed controller input version of RobotDrive (see previous comments).
     * @param rearLeftMotor The back left SpeedController object used to drive the robot.
     * @param frontLeftMotor The front left SpeedController object used to drive the robot
     * @param rearRightMotor The back right SpeedController object used to drive the robot.
     * @param frontRightMotor The front right SpeedController object used to drive the robot.
     */
    public RobotDrivePlus(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
            SpeedController frontRightMotor, SpeedController rearRightMotor) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor, kDefaultSensitivity);
    }

	/**
     * Arcade drive implements single stick driving.
     * This function lets you directly provide joystick values from any source.
     * @param moveValue The value to use for fowards/backwards
     * @param rotateValue The value to use for the rotate right/left
     * @param squaredInputs If set, increases the sensitivity at low speeds
     */
    public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
        // local variables to hold the computed PWM values for the motors
        double leftMotorSpeed;
        double rightMotorSpeed;

        moveValue = limit(moveValue);
        rotateValue = limit(rotateValue);

        if (squaredInputs) {
            // square the inputs (while preserving the sign) to increase fine control while permitting full power
            if (moveValue >= 0.0) {
                moveValue = (moveValue * moveValue);
            } else {
                moveValue = -(moveValue * moveValue);
            }
            if (rotateValue >= 0.0) {
                rotateValue = (rotateValue * rotateValue);
            } else {
                rotateValue = -(rotateValue * rotateValue);
            }
        }

        if (moveValue > 0.0) {
            if (rotateValue > 0.0) {
                leftMotorSpeed = (moveValue - rotateValue);
                rightMotorSpeed = Math.max(moveValue, rotateValue);
            } else {
                leftMotorSpeed = Math.max(moveValue, -rotateValue);
                rightMotorSpeed = (moveValue + rotateValue);
            }
        } else {
            if (rotateValue > 0.0) {
                leftMotorSpeed = -Math.max(-moveValue, rotateValue);
                rightMotorSpeed = (moveValue + rotateValue);
            } else {
                leftMotorSpeed = (moveValue - rotateValue);
                rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
            }
			double tmp = leftMotorSpeed;
			leftMotorSpeed = rightMotorSpeed;
			rightMotorSpeed = tmp;
        }
        setLeftRightMotorSpeeds(leftMotorSpeed, rightMotorSpeed);

    }

	/**
     * Limit motor values to the -1.0 to +1.0 range.
     */
    private double limit(double num) {
        if (num > 1.0) {
            return 1.0;
        }
        if (num < -1.0) {
            return -1.0;
        }
        return num;
    }
}
